// Waypoint.cpp: implementation of the Waypoint class.
//
//////////////////////////////////////////////////////////////////////

#include "Waypoint.h"

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

Waypoint::Waypoint(char filename[],float x_correction, float y_correction)
{
FILE* f;
float n,e;
char c;

this->x_correction = x_correction;
this->y_correction = y_correction;

// create a new ulLinkedList
this->waypointList = new ulLinkedList();

// read coords from file
if( (f = fopen(filename,"r")) != NULL)
	{
	while( fscanf(f,"%f%f%c",&e,&n,&c) != EOF)
		{
		WPCoord *wpc = (WPCoord*)malloc(sizeof(WPCoord));
		wpc->e = e;
		wpc->n = n;
		wpc->h = -9999.0f;

		this->waypointList->appendNode(wpc);
		}
	fclose(f);
	}
else
	{
	printf("Cannot open waypoint file %s for reading.\n",filename);
	}

}

Waypoint::~Waypoint()
{

}

void Waypoint::create(ssgRoot *root)
{
// load main points 3D model
e = ssgLoadAC(WAYPOINTS_MODEL_NAME);
e->setName("WAYPOINT\0");

// create a ssgTransfrom node
t = new ssgTransform();
t->setName("WAYPOINT_TRACK\0");

ssgTransform *lt;
// create a transform for each waypoint!!!
int n = this->waypointList->getNumNodes();

for(int i = 0; i < n; i++)
	{
	WPCoord *wpc = (WPCoord*)waypointList->getNodeData(i);
	wpc->h = getHeigth(wpc->e + x_correction,wpc->n+ y_correction,root);

	/* Next waypoint */
	WPCoord *wpc2 = NULL;
	WPCoord *newWPC = wpc;
	WPCoord *newWPC2 = NULL;
	if(i < (n-1)) 
		{
		wpc2 = (WPCoord*)waypointList->getNodeData(i+1);
		wpc2->h = getHeigth(wpc2->e + x_correction,wpc2->n+ y_correction,root);
		
		// calculate distance between two main waypoints
		sgVec3 p1;
		sgVec3 p2;
		
		sgSetVec3(p1,wpc->e,wpc->n,wpc->h);
		sgSetVec3(p2,wpc2->e,wpc2->n,wpc2->h);

		int color;

		if(wpc->h < wpc2->h)
			color = 0;
		else color = 1;

		float distance = sgDistanceVec3(p1,p2);

		// how many interpolated points will there be???
		int wpnumber = (int)(distance / MIN_DISTANCE_BETWEEN_WAYPOINTS);

		// whats the real distance between waypoints???
		float delta_distance = distance / (float)wpnumber;

		int less;
		if( (distance - delta_distance) == 0.0f ) less = 1;
		else less = 0;
	
		lt = new ssgTransform();
		lt->setName("LINE_TRANSFORM\0");
		if(wpnumber == 0)
			{
			createLine(lt,wpc,wpc2,color);
			//printf("MIN DISTANCE!!! %f <= %f\n",distance,MIN_DISTANCE_BETWEEN_WAYPOINTS);
			t->addKid(lt);
			//lt->print(stdout," ",2);
			wpc2->line_t = lt;
			}
		else
			{
			//printf("Distance : %f\n",distance);
		// calculate the interpolated point
			for (int nn = 0; nn < wpnumber - less; nn++) // less = 1 OK
				{
				float delta = (delta_distance * (nn+1))/distance;
				newWPC2 = createLERPWaypoint(wpc,wpc2,delta);
				newWPC2->h = getHeigth(newWPC2->e + x_correction,newWPC2->n+ y_correction,root);
				/* 3D LINES CREATION */
				createLine(lt,newWPC,newWPC2,color);
				if(newWPC != wpc) free(newWPC);
				newWPC = newWPC2;
				newWPC2 = NULL;
				}
			t->addKid(lt);
			wpc2->line_t = lt;
			}
		//t->addKid(lt);

		}

	/* WAYPOINTS CREATION */
	ssgTransform* wt = new ssgTransform;
	wt->setName("WAYPOINT_TRANSFORM\0");
	sgCoord tc;
	sgSetCoord(&tc,wpc->e + x_correction,wpc->n + y_correction,wpc->h,0.0f,0.0f,0.0f);
	if(i == 0)
		{
		wpc->waypoint_t = wt;
		wpc->line_t = NULL;
		}
	else
		{
		wpc->waypoint_t = wt;
		//wpc->line_t = lt;
		}
	wt->setTransform(&tc);
	wt->addKid(e);
	t->addKid(wt);
	}
root->addKid(t);
//printf("LINES: %d\n",t->getNumKids());
}

void Waypoint::add(ssgRoot* root, float x, float y, float z)
{
int num = this->waypointList->getNumNodes();

if(num > 0)
	{
	// retrieve last node
	WPCoord* wpc = (WPCoord*)waypointList->getNodeData(num-1);
	WPCoord* wpc2 = (WPCoord*)malloc(sizeof(WPCoord));
	wpc2->e = x - x_correction;
	wpc2->n = y - y_correction;
	wpc2->h = getHeigth(wpc2->e + x_correction,wpc2->n+ y_correction,root);
	this->waypointList->appendNode(wpc2);

	// calculate distance between two main waypoints
	sgVec3 p1;
	sgVec3 p2;

	sgSetVec3(p1,wpc->e,wpc->n,wpc->h);
	sgSetVec3(p2,wpc2->e,wpc2->n,wpc2->h);


	int color;

	if(wpc->h < wpc2->h)
		color = 0;
	else color = 1;

	float distance = sgDistanceVec3(p1,p2);

	// how many interpolated points will there be???
	int wpnumber = (int)(distance / MIN_DISTANCE_BETWEEN_WAYPOINTS);

	// whats the real distance between waypoints???
	float delta_distance = distance / (float)wpnumber;

	int less;
	if( (distance - delta_distance) == 0.0f ) less = 1;
	else less = 0;

	ssgTransform *lt = new ssgTransform;
	lt->setName("LINE_TRANSFORM\0");

	if(wpnumber == 0)
		{
		printf("MIN DISTANCE!!! %f <= %f\n",distance,MIN_DISTANCE_BETWEEN_WAYPOINTS);
		createLine(lt,wpc,wpc2,color);
		t->addKid(lt);
		}
	else
		{
		printf("Distance : %f\n",distance);
		WPCoord *newWPC = wpc;
		WPCoord *newWPC2 = NULL;
		// calculate the interpolated point
		for (int nn = 0; nn < wpnumber - less; nn++) // less = 1 OK
			{
			float delta = (delta_distance * (nn+1))/distance;
			newWPC2 = createLERPWaypoint(wpc,wpc2,delta);
			newWPC2->h = getHeigth(newWPC2->e + x_correction,newWPC2->n+ y_correction,root);
			/* 3D LINES CREATION */
			createLine(lt,newWPC,newWPC2,color);
			if(newWPC != wpc) free(newWPC);
			newWPC = newWPC2;
			}
		t->addKid(lt);
		}
	
	/* WAYPOINTS CREATION */
	ssgTransform* wt = new ssgTransform;
	wt->setName("WAYPOINT_TRANSFORM\0");
	sgCoord tc;
	sgSetCoord(&tc,wpc2->e + x_correction,wpc2->n + y_correction,wpc2->h,0.0f,0.0f,0.0f);
	wpc2->waypoint_t = wt;
	wpc2->line_t = lt;
	wt->setTransform(&tc);
	wt->addKid(e);
	t->addKid(wt);
	}
else
	{
	WPCoord* wpc0 = (WPCoord*)malloc(sizeof(WPCoord));
	wpc0->e = x - x_correction;
	wpc0->n = y - y_correction;
	wpc0->h = getHeigth(wpc0->e + x_correction,wpc0->n+ y_correction,root);
	this->waypointList->appendNode(wpc0);
	/* WAYPOINTS CREATION */
	ssgTransform* wt = new ssgTransform;
	wt->setName("WAYPOINT_TRANSFORM\0");
	sgCoord tc;
	sgSetCoord(&tc,wpc0->e + x_correction,wpc0->n + y_correction,wpc0->h,0.0f,0.0f,0.0f);
	wpc0->waypoint_t = wt;
	wpc0->line_t = NULL;
	wt->setTransform(&tc);
	wt->addKid(e);
	t->addKid(wt);
	}
}

void Waypoint::insert(ssgRoot* root, int bp, int ap, float x, float y, float z)
{
// retrieve last node
WPCoord* before = (WPCoord*)waypointList->getNodeData(bp);
WPCoord* after  = (WPCoord*)waypointList->getNodeData(ap);

this->t->removeKid(after->line_t);

WPCoord* selected = (WPCoord*)malloc(sizeof(WPCoord));

selected->e = x - x_correction;
selected->n = y - y_correction;
selected->h = getHeigth(selected->e + x_correction,selected->n+ y_correction,root);

this->waypointList->insertNode(selected,ap);

// calculate distance between two main waypoints
sgVec3 p1;
sgVec3 p2;

sgSetVec3(p1,before->e,before->n,before->h);
sgSetVec3(p2,selected->e,selected->n,selected->h);


int color;

if(before->h < selected->h)
	color = 0;
else color = 1;

float distance = sgDistanceVec3(p1,p2);

// how many interpolated points will there be???
int wpnumber = (int)(distance / MIN_DISTANCE_BETWEEN_WAYPOINTS);

// whats the real distance between waypoints???
float delta_distance = distance / (float)wpnumber;

int less;
if( (distance - delta_distance) == 0.0f ) less = 1;
else less = 0;

ssgTransform *lt = new ssgTransform;
lt->setName("LINE_TRANSFORM\0");

if(wpnumber == 0)
	{
	printf("MIN DISTANCE!!! %f <= %f\n",distance,MIN_DISTANCE_BETWEEN_WAYPOINTS);
	createLine(lt,before,selected,color);
	t->addKid(lt);
	}
else
	{
	printf("Distance : %f\n",distance);
	WPCoord *newWPC = before;
	WPCoord *newWPC2 = NULL;
	// calculate the interpolated point
	for (int nn = 0; nn < wpnumber - less; nn++) // less = 1 OK
		{
		float delta = (delta_distance * (nn+1))/distance;
		newWPC2 = createLERPWaypoint(before,selected,delta);
		newWPC2->h = getHeigth(newWPC2->e + x_correction,newWPC2->n+ y_correction,root);
		/* 3D LINES CREATION */
		createLine(lt,newWPC,newWPC2,color);
		if(newWPC != before) free(newWPC);
		newWPC = newWPC2;
		}
	t->addKid(lt);
	}
selected->line_t = lt;

sgSetVec3(p1,selected->e,selected->n,selected->h);
sgSetVec3(p2,after->e,after->n,after->h);

if(selected->h < after->h)
	color = 0;
else color = 1;

distance = sgDistanceVec3(p1,p2);

// how many interpolated points will there be???
wpnumber = (int)(distance / MIN_DISTANCE_BETWEEN_WAYPOINTS);

// whats the real distance between waypoints???
delta_distance = distance / (float)wpnumber;

if( (distance - delta_distance) == 0.0f ) less = 1;
else less = 0;

ssgTransform *lt2 = new ssgTransform;
lt2->setName("LINE_TRANSFORM\0");

if(wpnumber == 0)
	{
	printf("MIN DISTANCE!!! %f <= %f\n",distance,MIN_DISTANCE_BETWEEN_WAYPOINTS);
	createLine(lt2,selected,after,color);
	t->addKid(lt2);
	}
else
	{
	printf("Distance : %f\n",distance);
	WPCoord *newWPC = selected;
	WPCoord *newWPC2 = NULL;
	// calculate the interpolated point
	for (int nn = 0; nn < wpnumber - less; nn++) // less = 1 OK
		{
		float delta = (delta_distance * (nn+1))/distance;
		newWPC2 = createLERPWaypoint(selected,after,delta);
		newWPC2->h = getHeigth(newWPC2->e + x_correction,newWPC2->n+ y_correction,root);
		/* 3D LINES CREATION */
		createLine(lt2,newWPC,newWPC2,color);
		if(newWPC != selected) free(newWPC);
		newWPC = newWPC2;
		}
	t->addKid(lt2);
	}
after->line_t = lt2;
/* WAYPOINTS CREATION */
ssgTransform* wt = new ssgTransform;
wt->setName("WAYPOINT_TRANSFORM\0");
sgCoord tc;
sgSetCoord(&tc,selected->e + x_correction,selected->n + y_correction,selected->h,0.0f,0.0f,0.0f);
selected->waypoint_t = wt;
selected->line_t = lt;
wt->setTransform(&tc);
wt->addKid(e);
t->addKid(wt);
}

WPCoord* Waypoint::createLERPWaypoint(WPCoord* w1, WPCoord *w2, float delta)
{
sgVec3 p1;
sgVec3 p2;
sgVec3 lerp;

sgSetVec3(p1,w1->e,w1->n,w1->h);
sgSetVec3(p2,w2->e,w2->n,w2->h);

sgLerpVec3(lerp,p1,p2,delta);

WPCoord *w3 = (WPCoord*)malloc(sizeof(WPCoord));
w3->e = lerp[SG_X];
w3->n = lerp[SG_Y];
w3->h = lerp[SG_Z]; // useless

return w3;
}

void Waypoint::createLine(ssgTransform* t, WPCoord *c1, WPCoord *c2, int color)
{
ssgTransform* lt = new ssgTransform();

sgCoord coord;
sgSetCoord(&coord,c1->e + x_correction,c1->n + y_correction,c1->h,0.0f,0.0f,0.0f);

lt->setTransform(&coord);

ssgVertexArray   *vl   = new ssgVertexArray(4);
ssgNormalArray   *nl   = new ssgNormalArray(4);
ssgColourArray   *cl   = new ssgColourArray(4);

// create line points (for GL_QUADS)
sgVec3 pointBL;
sgVec3 pointTL;
sgVec3 pointBR;
sgVec3 pointTR;

sgSetVec3(pointBL,0.0f,0.0f,0.0f);
sgSetVec3(pointTL,0.0f,0.0f,WAYPOINT_LINE_HEIGTH);
sgSetVec3(pointBR,(c2->e + x_correction) - (c1->e + x_correction),(c2->n + y_correction) - (c1->n + y_correction),c2->h - c1->h);
sgSetVec3(pointTR,(c2->e + x_correction) - (c1->e + x_correction),(c2->n + y_correction) - (c1->n + y_correction),c2->h - c1->h + WAYPOINT_LINE_HEIGTH);

// make normals
sgVec3 normBL;
sgVec3 normBR;
sgVec3 normTL;
sgVec3 normTR;
sgVec3 norm;

sgMakeNormal(normBL,pointBL,pointBR,pointTL);	//sgNormaliseVec3(normBL);
sgMakeNormal(normBR,pointBR,pointTR,pointBL);   //sgNormaliseVec3(normBR);
sgMakeNormal(normTL,pointTL,pointBL,pointTR);   //sgNormaliseVec3(normTL);
sgMakeNormal(normTR,pointTR,pointTL,pointBR);   //sgNormaliseVec3(normTR);
sgMakeNormal(norm,pointBL,pointTL,pointBR);

// choose colors (RED is down, GREEN is up)
// create colors (one per vertex)
sgVec4 colorBL;
sgVec4 colorBR;
sgVec4 colorTL;
sgVec4 colorTR;

if( color == 0 )
	{
	sgSetVec4(colorBL,LINE_GREEN);
	sgSetVec4(colorTL,LINE_GREEN);
	sgSetVec4(colorBR,LINE_GREEN);
	sgSetVec4(colorTR,LINE_GREEN);
	}
else 
	{
	sgSetVec4(colorBL,LINE_RED);
	sgSetVec4(colorTL,LINE_RED);
	sgSetVec4(colorBR,LINE_RED);
	sgSetVec4(colorTR,LINE_RED);
	}

vl->add(pointBL);
vl->add(pointBR);
vl->add(pointTR);
vl->add(pointTR);
vl->add(pointTL);
vl->add(pointBL);

nl->add(normBL);
nl->add(normBR);
nl->add(normTR);
nl->add(normTR);
nl->add(normTL);
nl->add(normBL);

cl->add(colorBL);
cl->add(colorBR);
cl->add(colorTR);
cl->add(colorTR);
cl->add(colorTL);
cl->add(colorBL);

vl->add(pointBL);
vl->add(pointTL);
vl->add(pointTR);
vl->add(pointTR);
vl->add(pointBR);
vl->add(pointBL);

nl->add(norm);
nl->add(norm);
nl->add(norm);
nl->add(norm);
nl->add(norm);
nl->add(norm);

cl->add(colorBL);
cl->add(colorTL);
cl->add(colorTR);
cl->add(colorTR);
cl->add(colorBR);
cl->add(colorBL);

// create a new vertex table
ssgVtxTable *leaf = new ssgVtxTable(GL_TRIANGLES,vl,nl,NULL,cl);
ssgSimpleState *s = new ssgSimpleState();
s->enable(GL_BLEND);
s->disable(GL_LIGHTING);
leaf->setState(s);

// set the leaf state
leaf -> setName  ("WAYPOINT_LINE");

// add the line transform
lt->addKid( leaf );

// attach it to the brach
t -> addKid( lt );
}

float Waypoint::getHeigth(float x, float y, ssgRoot *root)
{
sgMat4 objectPositionInverseMatrix;
sgMakeIdentMat4 ( objectPositionInverseMatrix ) ;

objectPositionInverseMatrix[3][SG_X] =  - (x);
objectPositionInverseMatrix[3][SG_Y] =  - (y);
objectPositionInverseMatrix[3][SG_Z] =  0.0f;

// Test vector for HOT: vector is perpendicular to the xy plane
sgVec3 testVectorHOT;

testVectorHOT[SG_X] = 0.0f;
testVectorHOT[SG_Y] = 0.0f;
testVectorHOT[SG_Z] = 100000.0f;

// ssgHit structure, it will contain the colliding triangles
ssgHit *hitResults;
int     numberOfHits;
float   hot = -9999.0f;

// Test database for HOT...
numberOfHits = ssgHOT ( root, testVectorHOT, objectPositionInverseMatrix, &hitResults );
			
// Read the results
for ( int k = 0 ; k < numberOfHits ; k++ )
	{
	ssgHit *h = &hitResults [ k ] ;
	
	float hgt = - h->plane[3] / h->plane[2] ;
	if ( hgt >= hot )
		{
		bool terrain = false;
		bool go = true;
		for(int p = 0; (p < h->getNumPathEntries()) && go; p++)
			{
			//printf("%s\n",h->getPathEntry(p)->getName());
			if(ulStrEqual(h->getPathEntry(p)->getName(),"LAND_TRANSFORM\0"))
				{
				terrain = true;
				hot = hgt ;
				}
			else if(ulStrEqual(h->getPathEntry(p)->getName(),"WAYPOINT_TRACK\0"))
				{
				terrain = false;
				go = false;
				}
			}
		}
	}

return hot;
}

void Waypoint::fprint(FILE* f)
{
int n = waypointList->getNumNodes();

for(int i = 0; i < n; i++)
	{
	WPCoord* wpc = (WPCoord*)waypointList->getNodeData(i);
	fprintf(f,"%f %f\n",wpc->e,wpc->n);
	}
}

WPCoord* Waypoint::get(ssgTransform* t, int what)
{
int n = this->waypointList->getNumNodes();
WPCoord* found = NULL;

for(int i = 0; i < n; i++)
	{
	WPCoord* wpc = (WPCoord*)waypointList->getNodeData(i);
	if(what == ARROW)
		{
		if(wpc->waypoint_t == t)
			found = wpc;
		}
	else if(what == LINE)
		{
		if(wpc->line_t == t)
			found = wpc;
		}
	}
return found;
}

int Waypoint::getPosition(WPCoord *wpc)
{
return waypointList->getNodePosition(wpc);
}

void Waypoint::remove(ssgRoot* root,WPCoord* wpc)
{
if(wpc != NULL)
	{
	int n = waypointList->getNumNodes();
	int selected_pos = waypointList->getNodePosition(wpc);
	int before_pos = selected_pos - 1;
	int after_pos = selected_pos + 1;

	WPCoord *selected = wpc;
	WPCoord *before   = NULL;
	WPCoord *after    = NULL;

	if( (before_pos < 0) ) 
		{
		if( (after_pos >= n) ) // selected waypoint was the first and only
			{
			//printf("FIRST AND ONLY ");
			t->removeKid(selected->waypoint_t);
			waypointList->removeNode(selected);
			}
		else // selected waypoint was the first
			{
			//printf("FIRST          ");
			after = (WPCoord*)waypointList->getNodeData(after_pos);
			t->removeKid(after->line_t);
			after->line_t = NULL;
			t->removeKid(selected->waypoint_t);
			waypointList->removeNode(selected);
			}
		}
	else if( (after_pos >= n) ) // selected waypoint was the last
		{
			t->removeKid(selected->line_t);
			t->removeKid(selected->waypoint_t);
			selected->line_t = NULL;
			selected->waypoint_t = NULL;
			waypointList->removeNode(selected);
		}
	else // selected waypoint was one of the middle
		{
			//printf("MIDDLE         ");
			before = (WPCoord*)waypointList->getNodeData(before_pos);
			after  = (WPCoord*)waypointList->getNodeData(after_pos);
			t->removeKid(selected->line_t);
			t->removeKid(after->line_t);
			t->removeKid(selected->waypoint_t);
			t->addKid(moveLine(root,before,after));
			waypointList->removeNode(selected);
		}

	//printf("repositioning waypoint (%d) from %.03f %.03f %.03f\n",selected_pos,wpc->e+x_correction,wpc->n+y_correction,wpc->h);
	//printf("                              to %.03f %.03f %.03f\n",x,y,z);
	}
}

void Waypoint::reposition(ssgRoot* root, WPCoord *wpc, float x, float y, float z)
{
if(wpc != NULL)
	{
	int n = waypointList->getNumNodes();
	int selected_pos = waypointList->getNodePosition(wpc);
	int before_pos = selected_pos - 1;
	int after_pos = selected_pos + 1;

	WPCoord *selected = wpc;
	WPCoord *before   = NULL;
	WPCoord *after    = NULL;

	if( (before_pos < 0) ) 
		{
		if( (after_pos >= n) ) // selected waypoint was the first and only
			{
			//printf("FIRST AND ONLY ");
			selected->e = x-x_correction;
			selected->n = y-y_correction;
			selected->h = z;
			sgCoord new_pos;
			sgSetCoord(&new_pos,x,y,z,0.0f,0.0f,0.0f);
			selected->waypoint_t->setTransform(&new_pos);
			}
		else // selected waypoint was the first
			{
			//printf("FIRST          ");
			after = (WPCoord*)waypointList->getNodeData(after_pos);
			selected->e = x-x_correction;
			selected->n = y-y_correction;
			selected->h = z;
			sgCoord new_pos;
			sgSetCoord(&new_pos,x,y,z,0.0f,0.0f,0.0f);
			selected->waypoint_t->setTransform(&new_pos);
			t->removeKid(after->line_t);
			t->addKid(moveLine(root,selected,after));
			}
		}
	else if( (after_pos >= n) ) // selected waypoint was the last
		{
			//printf("LAST           ");
			before = (WPCoord*)waypointList->getNodeData(before_pos);
			selected->e = x-x_correction;
			selected->n = y-y_correction;
			selected->h = z;
			sgCoord new_pos;
			sgSetCoord(&new_pos,x,y,z,0.0f,0.0f,0.0f);
			selected->waypoint_t->setTransform(&new_pos);
			t->removeKid(selected->line_t);
			t->addKid(moveLine(root,before,selected));
		}
	else // selected waypoint was one of the middle
		{
			//printf("MIDDLE         ");
			before = (WPCoord*)waypointList->getNodeData(before_pos);
			after  = (WPCoord*)waypointList->getNodeData(after_pos);
			selected->e = x-x_correction;
			selected->n = y-y_correction;
			selected->h = z;
			sgCoord new_pos;
			sgSetCoord(&new_pos,x,y,z,0.0f,0.0f,0.0f);
			selected->waypoint_t->setTransform(&new_pos);
			t->removeKid(selected->line_t);
			t->removeKid(after->line_t);
			t->addKid(moveLine(root,before,selected));
			t->addKid(moveLine(root,selected,after));
		}

	//printf("repositioning waypoint (%d) from %.03f %.03f %.03f\n",selected_pos,wpc->e+x_correction,wpc->n+y_correction,wpc->h);
	//printf("                              to %.03f %.03f %.03f\n",x,y,z);
	}
}

ssgTransform* Waypoint::moveLine(ssgRoot* root, WPCoord *wpc, WPCoord *wpc2)
{
// calculate distance between two main waypoints
sgVec3 p1;
sgVec3 p2;

sgSetVec3(p1,wpc->e,wpc->n,wpc->h);
sgSetVec3(p2,wpc2->e,wpc2->n,wpc2->h);
int color;

if(wpc->h < wpc2->h)
	color = 0;
else color = 1;

float distance = sgDistanceVec3(p1,p2);

// how many interpolated points will there be???
int wpnumber = (int)(distance / MIN_DISTANCE_BETWEEN_WAYPOINTS);

// whats the real distance between waypoints???
float delta_distance = distance / (float)wpnumber;

int less;
if( (distance - delta_distance) == 0.0f ) less = 1;
else less = 0;

ssgTransform *lt = new ssgTransform();
lt->setName("LINE_TRANSFORM\0");

if(wpnumber == 0)
	{
	printf("MIN DISTANCE!!! %f <= %f\n",distance,MIN_DISTANCE_BETWEEN_WAYPOINTS);
	createLine(lt,wpc,wpc2,color);
	}
else
	{
	printf("Distance : %f\n",distance);
	/* Next waypoint */
	WPCoord *newWPC = wpc;
	WPCoord *newWPC2 = NULL;
	// calculate the interpolated point
	for (int nn = 0; nn < wpnumber - less; nn++) // less = 1 OK
		{
		float delta = (delta_distance * (nn+1))/distance;
		newWPC2 = createLERPWaypoint(wpc,wpc2,delta);
		newWPC2->h = getHeigth(newWPC2->e + x_correction,newWPC2->n+ y_correction,root);
		/* 3D LINES CREATION */
		createLine(lt,newWPC,newWPC2,color);
		if(newWPC != wpc) free(newWPC);
		newWPC = newWPC2;
		}
	}
wpc2->line_t = lt;
return lt;
}

void Waypoint::hideWaypoints()
{

e->clrTraversalMaskBits(SSGTRAV_CULL);

}
